#include "motor.h"

void motor_init()
{
	PORTC.DIRSET |= PIN4_bm;
	PORTD.DIRSET |= PIN4_bm;

	TCC1.CTRLB = TC_WGMODE_SS_gc;
	TC_SetPeriod(&TCC1, 1000);
    
	TCD1.CTRLB = TC_WGMODE_SS_gc;
    TC_SetPeriod(&TCD1, 1000);

	TC_SetCount(&TCD1, 500);

	TC1_ConfigClockSource(&TCC1, TC_CLKSEL_DIV256_gc);
    TC1_ConfigClockSource(&TCD1, TC_CLKSEL_DIV256_gc);
}

void left_motor_on(int duty_cycle)
{
	TC_SetCompareA(&TCC1, duty_cycle);	// Set duty cycle
	TCC1.CTRLB |= TC1_CCAEN_bm;			// Enable the CC channel
}

void left_motor_off()
{
	TCC1.CTRLB &= ~TC1_CCAEN_bm;		// Disable the CC channel
}

void right_motor_on(int duty_cycle)
{
	TC_SetCompareA(&TCD1, duty_cycle);	// Set duty cycle
    TCD1.CTRLB |= TC1_CCAEN_bm;  		// Enable the CC channel
}

void right_motor_off()
{
	TCD1.CTRLB &= ~TC1_CCAEN_bm;		// Disable the CC channel
}
